// RT-Thread头文件
#include <rtthread.h>
#define DBG_TAG "main"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>
#include <rtdevice.h>
#include <board.h>

// HAL库头文件
#include "main.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"
#include "rt_thread_demo.h"
#include "../hardware/USART_ALL/usart_all.h"        // Serial
#include "../hardware/JY61P/JY61P.h"                // JY61P

// 全局变量--储存陀螺仪数据
uint8_t angle[10]={0};
uint8_t gyro[10]={0};
uint8_t acc[10]={0};

float pitch,roll,yaw;
float gyrox,gyroy,gyroz;
float accx,accy,accz;


/****************************** JY61P_Init BEGIN *******************************
 * 函    数：JY61P_Init
 * 功    能：初始化JY61P传感器
 * 参    数：无
 * 返 回 值：无
 * 注意事项：无
 ******************************************************************************/
void JY61P_Init(void)
{
    usart5_init(115200);  // 串口5初始化
    return;
}
/****************************** JY61P_Init END *********************************/

/****************************** 数据更新函数 BEGIN *******************************
 * 函    数：angle_update, gyro_update, acc_update
 * 功    能：更新相关传感器数据
 * 参    数：无
 * 返 回 值：无
 * 注意事项：无
 ******************************************************************************/
void angle_update(void)
{
	if((int)angle[8] != (0x55 + 0x53 + (int)angle[0] + (int)angle[1] + (int)angle[2] + (int)angle[3] + (int)angle[4] + (int)angle[5] + (int)angle[6] + (int)angle[7]) % 0x100)
		return;
	pitch = (float)((short)((short)angle[1]<<8|angle[0]))*180/32768;
	roll  = (float)((short)((short)angle[3]<<8|angle[2]))*180/32768;
	yaw   = (float)((short)((short)angle[5]<<8|angle[4]))*180/32768;
}

void gyro_update(void)
{
	if((int)angle[8] != (0x55 + 0x53 + (int)angle[0] + (int)angle[1] + (int)angle[2] + (int)angle[3] + (int)angle[4] + (int)angle[5] + (int)angle[6] + (int)angle[7]) % 0x100)
		return;
	pitch = (float)((short)((short)angle[1]<<8|angle[0]))*180/32768;
	roll  = (float)((short)((short)angle[3]<<8|angle[2]))*180/32768;
	yaw   = (float)((short)((short)angle[5]<<8|angle[4]))*180/32768;
}

void acc_update(void)
{
	if((int)acc[8] != (0x55 + 0x51 + (int)acc[0] + (int)acc[1] + (int)acc[2] + (int)acc[3] + (int)acc[4] + (int)acc[5] + (int)acc[6] + (int)acc[7]) % 0x100)
		return;
	accx = (float)((short)((short)acc[1]<<8|acc[0]))*16*9.8/32768;
	accy = (float)((short)((short)acc[3]<<8|acc[2]))*16*9.8/32768;
	accz = (float)((short)((short)acc[5]<<8|acc[4]))*16*9.8/32768;
}
/****************************** 数据更新函数 END ********************************/

/****************************** JY_push BEGIN **********************************
 * 函    数：JY_push
 * 功    能：接收JY61P传感器数据，解析数据包，存储到对应的数组中
 * 参    数：rx_data - 接收到的字节数据
 * 返 回 值：0 - 未完成
 *           JY61P_ANGLE_ID/JY61P_GYRO_ID/JY61P_ACC_ID - 对应类型数据包接收完成
 * 注意事项：接收完成后返回数据类型，便于上层区分
 ******************************************************************************/
uint8_t JY_push(uint8_t rx_data)
{
    static uint8_t RxState = 0;         // 状态机状态
    static uint8_t pRxPacket = 0;       // 当前数据包内字节计数
    static uint8_t* pRxArray = NULL;    // 当前数据存储指针
    static uint8_t last_type = 0;       // 记录当前数据类型

    switch (RxState)
    {
        case 0: // 等待帧头
            if (rx_data == JY61P_FRAME_HEADER)
            {
                RxState = 1;
                pRxPacket = 0;
            }
            break;
        case 1: // 判断数据类型
            if (rx_data == JY61P_ANGLE_ID)
            {
                pRxArray = angle;
                last_type = JY61P_ANGLE_ID;
                RxState = 2;
            }
            else if (rx_data == JY61P_GYRO_ID)
            {
                pRxArray = gyro;
                last_type = JY61P_GYRO_ID;
                RxState = 2;
            }
            else if (rx_data == JY61P_ACC_ID)
            {
                pRxArray = acc;
                last_type = JY61P_ACC_ID;
                RxState = 2;
            }
            else
            {
                RxState = 0; // 类型错误，复位状态机
            }
            break;
        case 2: // 接收数据
            if (pRxArray != NULL)
            {
                pRxArray[pRxPacket++] = rx_data;
                if (pRxPacket >= JY61P_FRAME_LEN)
                {
                    RxState = 0;
                    pRxArray = NULL;
                    return last_type; // 返回数据类型
                }
            }
            else
            {
                RxState = 0; // 指针异常，复位
            }
            break;
        default:
            RxState = 0;
            break;
    }
    return 0;
}
/****************************** JY_push END ************************************/



